import numpy as np
import cv2
import time
import math
import matplotlib.pyplot as plt
controus_list = []

# 得到向量的坐标以及向量的模长
class Point(object):
    def __init__(self, cx, cy, cx3, cy3):
        self.cx = cx
        self.cy = cy
        self.cx3 = cx3
        self.cy3 = cy3

    def vector(self):
        c = (self.cx - self.cx3, self.cy - self.cy3)
        return c

    def length(self):
        d = math.sqrt(pow((self.cx - self.cx3), 2) + pow((self.cy - self.cy3), 2))
        return d


# 计算向量夹角
class Calculate(object):
    def __init__(self, x, y, m, n):
        self.x = x
        self.y = y
        self.m = m
        self.n = n

    def Vector_multiplication(self):
        self.mu = np.dot(self.x, self.y)
        return self.mu

    def Vector_model(self):
        self.de = self.m * self.n
        return self.de

    def cal(self):
        result = Calculate.Vector_multiplication(self) / Calculate.Vector_model(self)
        return int(math.acos(result) / math.pi * 180)


class Detection(object):
    '''
        contours:存放每个轮廓的信息
    '''

    def __init__(self):
        self.contours = []

    def detecte(self, image):
        '''提取所有轮廓'''
        # gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)#灰度图
        # _, gray = cv2.threshold(gray, 180, 255, cv2.THRESH_BINARY)#二值膨胀
        # _, gray = cv2.threshold(gray, 250, 255, cv2.THRESH_OTSU)#二值膨胀

        dst = cv2.GaussianBlur(image, (3, 3), 0)  # 高斯模糊去噪
        gray = cv2.cvtColor(dst, cv2.COLOR_RGB2GRAY)
        ret, binary = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY | cv2.THRESH_OTSU)  # 用大律法、全局自适应阈值方法进行图像二值化

        # dst_size = (960, 540)
        # binary = cv2.resize(binary, dst_size)
        # dst = cv2.resize(dst, dst_size)

        self.contours, hierachy = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        print(len(self.contours))

        # print("time:{}".format(time.time()-con_time))
        cv2.imshow("gray", binary)
        cv2.imshow("image", dst)

        cv2.waitKey(1)
        return image, self.contours, hierachy

    def compute_1(self, i, j):
        '''回字三个轮廓比例为7：5：3，面积比为49：25：9'''
        '''最外面的轮廓和子轮廓的比例'''
        area1 = cv2.contourArea(self.contours[i])
        area2 = cv2.contourArea(self.contours[j])
        if area2 == 0:
            return False
        ratio = area1 * 1.0 / area2
        if abs(ratio - 49.0 / 25) > 1:
            return False
        return True

    def compute_2(self, i, j):
        '''回字三个轮廓比例为7：5：3，面积比为49：25：9'''
        '''子轮廓和子子轮廓的比例'''
        area1 = cv2.contourArea(self.contours[i])
        area2 = cv2.contourArea(self.contours[j])
        if area2 == 0:
            return False
        ratio = area1 * 1.0 / area2
        if abs(ratio - 25.0 / 9) > 1:
            return False

        return True

    def compute_3(self, i, j):
        '''外轮廓比例为4：3，面积比为16：9'''
        '''子轮廓和子子轮廓的比例'''
        area1 = cv2.contourArea(self.contours[i])
        area2 = cv2.contourArea(self.contours[j])

        if area2 == 0:
            return False
        ratio = area1 * 1.0 / area2
        if abs(ratio - 16.0 / 9) > 0.2:
            # print(area1, area2)
            return False
        return True

    def compute_center(self, i):
        '''计算轮廓中心点'''
        M = cv2.moments(self.contours[i])
        cx = int(M['m10'] / M['m00'])
        cy = int(M['m01'] / M['m00'])
        return cx, cy

    def detect_contours(self, vec):
        '''判断这个轮廓和它的子轮廓以及子子轮廓的中心的间距是否足够小'''
        distance_1 = np.sqrt((vec[0] - vec[2]) ** 2 + (vec[1] - vec[3]) ** 2)
        distance_2 = np.sqrt((vec[0] - vec[4]) ** 2 + (vec[1] - vec[5]) ** 2)
        distance_3 = np.sqrt((vec[2] - vec[4]) ** 2 + (vec[3] - vec[5]) ** 2)
        if sum((distance_1, distance_2, distance_3)) / 3 < 3:
            return True
        return False

    def detect_center(self, cx, cy, cx1, cy1):
        '''判断是不是两个中心点间距是否足够小'''
        distance_1 = np.sqrt((cx - cx1) ** 2 + (cy - cy1) ** 2)
        if distance_1 > 5:
            return True
        return False


def find(image):
    '''找到符合要求的轮廓'''
    Contour = Detection()
    c_index = 0
    image, contours, hierachy = Contour.detecte(image)
    controus_list.append(len(contours))
    hierachy = np.squeeze(hierachy)
    rec = []
    outer = []
    Dire = ""
    for i in range(len(hierachy)):
        child = hierachy[i][2]
        child_child = hierachy[child][2]
        child_child_child = hierachy[child_child][2]
        if child != -1 and hierachy[child][2] != -1:
            if child_child_child != -1 and hierachy[child_child_child][2] != -1:
                if Contour.compute_1(child, child_child) and Contour.compute_2(child_child, child_child_child):
                    cx1, cy1 = Contour.compute_center(i)
                    cx2, cy2 = Contour.compute_center(child)
                    cx3, cy3 = Contour.compute_center(child_child)
                    if Contour.detect_contours([cx1, cy1, cx2, cy2, cx3, cy3]):
                        rec.append([cx1, cy1, cx2, cy2, cx3, cy3, i, child, child_child])
                elif Contour.compute_3(i, child_child):  # 方框比例4:3'''检测kt板大外框'''
                    cx1, cy1 = Contour.compute_center(i)
                    cx2, cy2 = Contour.compute_center(child)
                    cx3, cy3 = Contour.compute_center(child_child)
                    if Contour.detect_contours([cx1, cy1, cx2, cy2, cx3, cy3]):
                        outer = [cx1, cy1, cx2, cy2, cx3, cy3, i, child, child_child]
                        break

    '''检测到大外框的时候，直接用大外框的的中心点'''
    if outer:
        cxz, cyz = Contour.compute_center(outer[6])
        cv2.drawContours(image, contours, outer[6], (0, 0, 255), 4)
        x, y, w, h = cv2.boundingRect(contours[i])
        # print(x,y,w,h)
        # cv2.line(image, (x, y), (x+w, y+h), (255, 0, 0), 2)
        cx1 = cxz
        cy1 = cyz - 3
        Dire = "None"
        return cxz, cyz, cx1, cy1, Dire

    '''计算得到所有在比例上符合要求的轮廓中心点，用juge_angle判断是否是三角形'''
    i, j, k = juge_angle(rec)
    if i == -1 or j == -1 or k == -1:
        return None
    '''再遍历一遍，把i、j、k三点去除。'''
    for r in range(len(hierachy)):
        child = hierachy[r][2]
        child_child = hierachy[child][2]
        if child != -1 and hierachy[child][2] != -1:
            if Contour.compute_1(r, child) and Contour.compute_2(child, child_child):
                cxz, cyz = Contour.compute_center(r)
                cx2, cy2 = Contour.compute_center(child)
                cx3, cy3 = Contour.compute_center(child_child)
                if Contour.detect_contours([cxz, cyz, cx2, cy2, cx3, cy3]):
                    cxi, cyi = Contour.compute_center(rec[i][6])  # i点的中心坐标，要与r点中心比较
                    cxj, cyj = Contour.compute_center(rec[j][6])  # j点的中心坐标，要与r点中心比较
                    cxk, cyk = Contour.compute_center(rec[k][6])  # k点的中心坐标，要与r点中心比较
                    '''确保r中心点不与i、j、k的中心点重合'''
                    if Contour.detect_center(cxi, cyi, cxz, cyz) and Contour.detect_center(cxj, cyj, cxz,
                                                                                           cyz) and Contour.detect_center(
                            cxk, cyk, cxz, cyz):
                        c_index = r

    '''得到i j k三个点，进行标注画线等操作; (cxz,cyz)为三角形中心的回字的中心坐标'''
    # 计算i、j、k的中心坐标
    if c_index:
        cxz, cyz = Contour.compute_center(c_index)
    cx1, cy1 = Contour.compute_center(rec[i][6])
    cx2, cy2 = Contour.compute_center(rec[j][6])
    cx3, cy3 = Contour.compute_center(rec[k][6])
    cx = int((abs(cx2) + abs(cx3)) / 2)
    cy = int((abs(cy2) + abs(cy3)) / 2)
    cxz1 = int((abs(cx1) + abs(cx)) / 2)  # cxz1,cyz1是根据三角形中心线计算出来的中心点，与中间回字的中心点(cxz,cyz)比较
    cyz1 = int((abs(cy1) + abs(cy)) / 2)
    '''不同中心点返回True同个中心点返回False'''
    if Contour.detect_center(cxz, cyz, cxz1, cyz1):
        return None  # 中线中心与基准点中心不重合，不是三角形，返回None
    cv2.drawContours(image, contours, c_index, (0, 255, 0), 2)
    cv2.drawContours(image, contours, rec[i][6], (255, 0, 0), 2)
    cv2.drawContours(image, contours, rec[j][6], (255, 0, 0), 2)
    cv2.drawContours(image, contours, rec[k][6], (255, 0, 0), 2)
    cv2.line(image, (cx, cy), (cx1, cy1), (0, 0, 255), 2)  # 要的线
    cv2.line(image, (cx1, cy1), (cx2, cy2), (255, 0, 0), 2)
    cv2.line(image, (cx1, cy1), (cx3, cy3), (255, 0, 0), 2)
    cv2.line(image, (cx2, cy2), (cx3, cy3), (255, 0, 0), 2)
    '''计算非等腰边的中心点'''
    if cx > cx1:
        Dire = "Left"
    elif cx < cx1:
        Dire = "Right"
    return cxz, cyz, cx1, cy1, Dire


def juge_angle(rec):
    '''判断寻找是否有三个点可以围成等腰直角三角形'''
    start_angle_time = time.time()
    if len(rec) < 3:
        return -1, -1, -1
    for i in range(len(rec)):
        for j in range(i + 1, len(rec)):
            for k in range(j + 1, len(rec)):
                # 三个点之间的距离
                distance_1 = np.sqrt((rec[i][0] - rec[j][0]) ** 2 + (rec[i][1] - rec[j][1]) ** 2)
                distance_2 = np.sqrt((rec[i][0] - rec[k][0]) ** 2 + (rec[i][1] - rec[k][1]) ** 2)
                distance_3 = np.sqrt((rec[j][0] - rec[k][0]) ** 2 + (rec[j][1] - rec[k][1]) ** 2)
                if abs(distance_1 - distance_2) < 10:  # 两边相减小于15（15为允许的误差），则认为两条边相等。
                    return i, j, k
                elif abs(distance_1 - distance_3) < 10:
                    return j, i, k
                elif abs(distance_2 - distance_3) < 10:
                    return k, i, j
    return -1, -1, -1


def camera(path):
    value = []
    FPS_list = []
    frame_count = 0

    capture = cv2.VideoCapture(path)
    Video_FPS = capture.get(cv2.CAP_PROP_FPS)
    height = int(capture.get(cv2.CAP_PROP_FRAME_HEIGHT))
    width = int(capture.get(cv2.CAP_PROP_FRAME_WIDTH))
    video = cv2.VideoWriter('./video/' + "p4.mp4", cv2.VideoWriter_fourcc('m', 'p', '4', 'v'), Video_FPS,
                            (width, height))
    (np.int(width), np.int(height), True)
    while True:
        ret, image = capture.read()
        frame_count += 1
        # if frame_count == 100:
        #     break
        if ret is True:
            # 镜像
            # image = cv2.resize(image,(1280,720))
            # t1 = int((width / 2) / 2)
            # t2 = int((height / 2) / 2)
            # image = image[t2:-t2, t1:-t1]
            # image = cv2.resize(image,(640,360))
            # print(width,height)
            start = time.time()
            image = cv2.flip(image, 1)
            width, height = image.shape[1], image.shape[0]
            res = find(image)
            fps = 1/(time.time()-start)
            FPS_list.append(fps)
            print("FPS:", fps,len(FPS_list),len(controus_list))
            from scatter import draw_sactter
            if not res:
                pass
            else:
                cx, cy, cx1, cy1, Dire = res
                first_point = Point(cx, cy, cx1, cy1)
                two_point = Point((width) / 2, height, (width) / 2, 0)
                ca = Calculate(first_point.vector(), two_point.vector(), first_point.length(), two_point.length())
                dx = cx - (width) / 2  # 目标中心点与屏幕中心点差值
                dy = cy - (height) / 2
                value = [dx, dy, Dire, ca.cal()]
                text = "dx:" + str(dx) + " dy:" + str(dy) + ' angle(' + Dire + "):" + str(ca.cal())
                cv2.line(image, (cx, cy), (int(width / 2), int(height / 2)), (0, 0, 255), 2)  # 目标中心点与屏幕中心点连线
                cv2.line(image, (0, int(height / 2)), (width, int(height / 2)), (0, 255, 0), 2)  # 屏幕中心交叉线
                cv2.line(image, (int(width / 2), 0), (int(width / 2), height), (0, 255, 0), 2)  # 屏幕中心交叉线
                cv2.putText(image, text, (20, 40), 1, 4, (0, 255, 0), 4)
                # print(value)
            video.write(image)
        else:

            break
            # cv2.imshow('img', image)
            # print(int(1/(time.time()-start)))
            # if cv2.waitKey(1) == 27:
            #     break  # esc to quit
    video.release()
    cv2.destroyAllWindows()
    draw_sactter(controus_list, FPS_list)


def singe_image(path):
    image = cv2.imread(path)
    width, height = image.shape[1], image.shape[0]
    res = find(image)
    if not res:
        return [0, 0, 0]
    else:
        cx, cy, cx1, cy1, Dire = res
        first_point = Point(cx, cy, cx1, cy1)
        two_point = Point((width) / 2, height, (width) / 2, 0)
        ca = Calculate(first_point.vector(), two_point.vector(), first_point.length(), two_point.length())
        dx = cx - (width) / 2
        dy = cy - (height) / 2
        value = [dx, dy, Dire, ca.cal()]
        # print(value)
        text = "dx:" + str(dx) + " dy:" + str(dy) + ' angle(' + Dire + "):" + str(ca.cal())
        cv2.putText(image, text, (20, 30), 1, 1, (0, 255, 0), 2)
    cv2.imshow('sb', image)
    cv2.waitKey(0)


if __name__ == '__main__':
    # path = "./DJI_0510.JPG"
    # singe_image(path)
    # while True:
    #     singe_image("./video/6.png")
    #     singe_image("./video/7.png")
    #     singe_image("./video/8.png")
    #     singe_image(path)
    camera("./test.mp4")  # 打开视频
